#include "HGraphPlanner.h"
#include "Conversion.h"
#include "MultiRobotPRM.h"
#include "Metrics.h"

//TODO: set the correct price
const int HGraphPlanner::GAP_EXT_PRICE = 150;
const int HGraphPlanner::MAX_VALUE = 10000000;

//#define USE_ALL_LINKS

HGraphPlanner::HGraphPlanner(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles, vector<Conf> &start_confs, vector<Conf> &target_confs, Room &room, bool measure, double alpha)
    : m_robot_polygons(robot_polygons), m_robot_num(robot_num), m_obstacles(obstacles), m_start_confs(start_confs), m_target_confs(target_confs), m_room(room), m_hgraph(0, true), m_measure(measure), m_alpha(alpha),
    m_mr_collision_detector(robot_polygons, robot_num, obstacles), m_detector(robot_polygons, m_robot_num, m_obstacles)
{
    
}

void HGraphPlanner::generate_roadmaps(int num_of_paths, int number_vertices, int k_nearest, double eps)
{
    //generate paths using prm
    for(int i = 0; i < num_of_paths; ++i){
        std::cout << "[HGraph]\tBuilding multi robot prm (" << i+1 << "\\" << num_of_paths << ")";

        MultiRobotPRM prm(m_robot_polygons, m_robot_num, m_obstacles, m_start_confs, m_target_confs, m_room, &m_mr_collision_detector);
        vector<vector<Conf>> path;

        prm.generate_roadmap(number_vertices, k_nearest, eps);
        prm.update_path(path);
        m_paths.push_back(path);
        if (path.size() > 0)
        {
            std::cout << " [T]";
        }
        else
        {
            std::cout << " [F]";
        }
        std::cout << " - Done!\n";
    }
}

double HGraphPlanner::update_path(vector<vector<Conf>> &path)
{
    Point_d start(Conversion::to_Point_d(m_start_confs));
    m_map[start] = m_start_confs;
    Point_d target(Conversion::to_Point_d(m_target_confs));
    m_map[target] = m_target_confs;
    m_hgraph.add_vertex(start);
    m_hgraph.add_vertex(target);

    //the vertices of the hgraph are all the nodes in paths.
    //the edges include the previous edges
    //the start and the target in all the paths are the same
    vector<vector<Point_d>> pointDPaths;
    BOOST_FOREACH(MPATH path, m_paths){
        if(path.empty()){
            continue;
        }

        vector<Point_d> pathD;
        Point_d prev = start;
        pathD.push_back(start);
        for(int i = 1; i <path.size()-1; ++i){
            Point_d p = Conversion::to_Point_d(path[i]);
            pathD.push_back(p);
            m_map[p] = path[i];
            m_hgraph.add_vertex(p);
            m_hgraph.add_edge(prev, p, getWeight(prev,p));
            prev = p;
        }
        pathD.push_back(target);
        m_hgraph.add_edge(prev, target, getWeight(prev, target));

        pointDPaths.push_back(pathD);
    }

    std::cout << "[HGraph]\tCombining paths";
#ifdef USE_ALL_LINKS
    std::cout << " [all]";
#else
    std::cout << " [smart]";
#endif
    for(int i=0; i<pointDPaths.size(); ++i){
        for(int j=i+1; j<pointDPaths.size(); ++j){
#ifdef USE_ALL_LINKS
            vector<Point_d> a = pointDPaths[i];
            vector<Point_d> b = pointDPaths[j];
            BOOST_FOREACH(Point_d& p_a, a)
            {
                BOOST_FOREACH(Point_d& p_b, b)
                {
                    bool isValid = m_mr_collision_detector.local_planner(m_map[p_a], m_map[p_b]);        
                    if (isValid){            
                        m_hgraph.add_edge(p_a, p_b, getWeight(p_a, p_b));
                    }
                }
            }
#else
            vector<vector<Point_d>> potentialBridgeEdges = getPotentialBridgeEdges(pointDPaths[i], pointDPaths[j]);
            BOOST_FOREACH(vector<Point_d> edge , potentialBridgeEdges){
                bool isValid = m_mr_collision_detector.local_planner(m_map[edge[0]], m_map[edge[1]]);        
                if (isValid){            
                    m_hgraph.add_edge(edge[0], edge[1], getWeight(edge[0], edge[1]));
                }
            }
#endif
        }
    }
    std::cout << " - Done!\n";

    //run dijkstra
    list<Point_d> bestPath;
    m_hgraph.find_path(start, target, bestPath);

    //translate to configuration space
    BOOST_FOREACH(Point_d p, bestPath){
        path.push_back(m_map[p]);
    }

    return measure(bestPath);
}

double HGraphPlanner::measure(list<Point_d>& path)
{
    double value = 0;
    if (path.size() == 0)
        return 0;
    list<Point_d>::iterator iter = path.begin();
    Point_d prev = *iter;
    ++iter;

    for (; iter != path.end(); ++iter)
    {
        value += getWeight(prev, *iter);
        prev = *iter;
    }
    return value;
}

void HGraphPlanner::calculateDynamicPathMatching(vector<Point_d> p, vector<Point_d> q){
    for(int i=0; i<p.size(); ++i){
        for(int j=0; j<q.size(); ++j){
            if(i==0 && j==0){
                m_costMatrix[make_pair(i,j)] = 0;
                //tODO: make some default value
                m_traceBackMatrix[make_pair(i,j)] = UP;
                continue;
            }

            int _match = MAX_VALUE;
            if( i!=0 && j!=0){
                _match = getDelta(p[i], q[j]);
                _match += m_costMatrix.at(make_pair(i-1, j-1));
            }

            int _up = MAX_VALUE;
            if(i != 0){
                _up = GAP_EXT_PRICE;
                _up += m_costMatrix.at(make_pair(i-1, j));
            }

            int _left = MAX_VALUE; 
            if( j != 0 ){
                _left = GAP_EXT_PRICE;
                _left += m_costMatrix.at(make_pair(i, j-1));
            }

            if(_match < _up){
                if (_left < _match){
                    m_traceBackMatrix[make_pair(i,j)] = LEFT;
                    m_costMatrix[make_pair(i,j)] = _left;
                } else{
                    m_traceBackMatrix[make_pair(i,j)] = MATCH;
                    m_costMatrix[make_pair(i,j)] = _match;
                }
            } else{
                if(_left < _up){
                    m_traceBackMatrix[make_pair(i,j)] = LEFT;
                    m_costMatrix[make_pair(i,j)] = _left;
                } else{
                    m_traceBackMatrix[make_pair(i,j)] = UP;
                    m_costMatrix[make_pair(i,j)] = _up;
                }
            }
        }
    }
}

void HGraphPlanner::printMatrices(int m, int n){
    /*
    ofstream myfile;
    myfile.open("costs.txt");
    for(int i = 0; i<m ; ++i){
        for(int j=0; j<n ; ++j){
            myfile << m_costMatrix[make_pair(i,j)] << "\t" ;
        }
        myfile << "\n" ;
    }
    myfile.flush();
    myfile.close();

    ofstream tfile;
    tfile.open("trace.txt");
    for(int i = 0; i<m ; ++i){
        for(int j=0; j<n ; ++j){
            tfile << m_traceBackMatrix[make_pair(i,j)] << "\t" ;
        }
        tfile << "\n" ;
    }
    tfile.flush();
    tfile.close();
    */
}

vector<vector<Point_d>> HGraphPlanner::getPotentialBridgeEdges(vector<Point_d> p , vector<Point_d> q){
    vector<vector<Point_d>> bridges;

    calculateDynamicPathMatching(p, q);

    //printMatrices(p.size(), q.size());
 
    // trace back and get all the additional edges
    bool isGapUp = false;
    bool isGapLeft = false;
    int i = p.size()-1;
    int j = q.size()-1;
    while(i!=0 || j!=0){
        MOVEMENT mov = m_traceBackMatrix[make_pair(i, j)];
        switch(mov){
        case(MATCH):
            {
                vector<Point_d> edge1;
                edge1.push_back(p[i]);
                edge1.push_back(q[j]);
                bridges.push_back(edge1);

                if(isGapUp){
                    isGapUp = false;
                    vector<Point_d> edge2;
                    edge2.push_back(p[i]);
                    edge2.push_back(q[j-1]);
                    bridges.push_back(edge2);
                }

                if (isGapLeft){
                    isGapLeft = false;
                    vector<Point_d> edge3;
                    edge3.push_back(p[i-1]);
                    edge3.push_back(q[j]);
                    bridges.push_back(edge3);
                }

                if(i!=0)
                    i--;
                if(j!=0)
                    j--;
                break;
            }
        case(UP):
            {
                if(! isGapUp){
                    isGapUp = true;
                    vector<Point_d> edge4;
                    edge4.push_back(p[i-1]);
                    edge4.push_back(q[j]);
                    bridges.push_back(edge4);
                }
                if( isGapLeft ){
                    isGapLeft = false;
                }
                if(i!=0)
                    i--;
                break;
            }
        case(LEFT):
            {
                if (! isGapLeft){
                    isGapLeft = true;
                    vector<Point_d> edge5;
                    edge5.push_back(p[i]);
                    edge5.push_back(q[j-1]);
                    bridges.push_back(edge5);
                }
                if( isGapUp ){
                    isGapUp = false;
                }
                if(j!=0){
                    j--;
                }
                break;
            }
        default: //we shouldnt be here..
            break;
        }

    }

    return bridges;
}

double HGraphPlanner::getDelta(Point_d p, Point_d q){
    double distance = Metrics::distance(p, q);
    return distance;
}

double HGraphPlanner::getWeight(Point_d p, Point_d q){
    if (!m_measure)
    {
        return getDelta(p, q);
    }
    double distance = Metrics::distance(p,q);
    double clearance = min(m_detector.clearance(p), m_detector.clearance(q)); //heuristic - doesn't find the exact min... it takes too long...
    double weight = distance/pow(clearance, m_alpha);
    return weight;
}
